# Incluyo archivo con funciones utiles
from util import *

# Incluyo modulo de visibilidad
import visilibity as vis
#from visilibity_test import *

# Incluyo las variables globales que voy a necesitar
from globalVar import GLOBAL_VAR, NUMERO, agentes, FLAG_DEBUGG, VISILIBITY


class Agente(pygame.sprite.Sprite):
    
    def __init__ (self, name=None):
        global NUMERO,GLOBAL_VAR, VISILIBITY
        # Inicializo posicion, velocidad, aceleracion y fuerza en (0,0)
        self.pos = (0,0)
        self.speed = [0,0]
        self.acel = [0,0]
        self.fuerza = [0,0]
        # Cargo numero de identificacion del agente
        self.id = NUMERO
        # Genero proximo numero de identificacion
        NUMERO += 1
        # Inicializo la masa del agente igual a 1
        self.masa = GLOBAL_VAR['masa']
        
        # Constantes de sensonr de vision
        self.rango_vision = VISILIBITY['range']
        self.amplitud_cono = VISILIBITY['amplitude']
        self.epsilon = VISILIBITY['epsilon']
        self.quality_cone = VISILIBITY['resolution']
        
        if GLOBAL_VAR['GRAFICO']:
            # Inicializo Sprite
            pygame.sprite.Sprite.__init__ (self) 
            
            if name == None:
                # Cargo nombre de imagen por defecto
                name = 'agente.jpg'
            
            # Cargo imagen y rectangulo de imagen
            self.image, self.rect = load_image(name)
            
            # Recupero la pantalla
            screen = pygame.display.get_surface()
            
            # Guardo area de la pantalla
            self.area = screen.get_rect()
            
            # Defino una escala para el renderizado
            self.escala = 25
            
            # Situo al agente en la posicion inicial
            self.rect.centerx, self.rect.centery = self.pos[0], self.pos[1]
    
    # Desc: Recibe dos coordenadas y situa al agente en esas coordenadas
    def situar_en(self, new_pos_x, new_pos_y):
        global GLOBAL_VAR
        # Actualizo posicion
        self.pos = (new_pos_x, new_pos_y)
        
        if GLOBAL_VAR['GRAFICO']:
            # Situo al agente en la posicion actual usando escala
            self.rect.centerx, self.rect.centery = self.posEscala()
    
    def sensor(self):
        
        # Llamo a la funcion para sensar
        sensado = self.sense()
        
        # Devuelvo lo que sense
        return sensado
    
    def sense(self):
        global agentes
        
        # Lista de agentes que puedo ver
        visibles = []
        
        # Llamo a la funcion de vision
        polygon = self.vision()
        
        for agente in agentes:
            
            # Chequeo que el agente no sea yo
            if agente.id == self.id :
                continue
            
            # Creo punto
            point = vis.Point(agente.pos[0], agente.pos[1])
            
            # Veo si el punto esta en el cono de vision
            #if vis.point_inside_polygon(point,polygon):
            if point._in(polygon,self.epsilon):
                #print "TODO OK"
                # Si veo al agente lo guardo en lista
                visibles.append(agente)
        
        # Guardo la lista de agentes que puedo ver
        self.visibles = visibles
        
        # Devuelvo la lista de agentes que puedo ver
        return visibles
    
    def sensarCA(self):
        global agentes
        ret = []
        
        for agente in agentes:
            if agente.id == self.id:
                continue
            
        
    
    def vision(self):
        
        # Defino el punto del cual observo
        observer = vis.Point(self.pos[0], self.pos[1])
        
        # Creo el ambiente
        env = self.vision_environment()
        
        observer.snap_to_boundary_of(env, self.epsilon)
        observer.snap_to_vertices_of(env, self.epsilon)
        
        # Genero poligono de vision
        polygon_vis = vis.Visibility_Polygon(observer, env, self.epsilon)
        
        # Devuelvo el poligono de vision
        return polygon_vis
    
    # Desc: genera el ambiente en el que vera el agente
    def vision_environment(self):
        
        # Calculo el angulo de la velocidad en radianes
        rad = math.atan2(self.speed[1], self.speed[0])
        
        # Paso el angulo a grados
        degrees = math.degrees(rad) % 360
        
        # creo cono de vision
        walls = self.cone_vision(degrees)
        
        # Creo el ambiente en que quiero que vea
        env = vis.Environment([walls])
        
        # Chequeo que el ambiente sea epsilon-valido
        # Para mejorar performance
        #if not env.is_valid(self.epsilon):
            
        #    # Si no lo es imprimo mensaje de error y salgo
        #    print 'Error:Vision: Ambiente no valido!'
        #    return -1
        
        # Devuelvo el ambiente
        return env
        
    def cone_vision(self, angulo_vision ):
        
        # Creo el poligono de vision. En este caso es un cono
        # cuyo radio es 'self.rango_vision', el angulo en el que 
        # esta orientado es 'angulo_vision' y la amplitud del arco
        # del cono es 2*self.amplitud_cono
        walls = vis.create_cone(self.pos, self.rango_vision, angulo_vision, 
                    self.amplitud_cono, quality= self.quality_cone)
        
        #if not walls.is_in_standard_form():
        #    print 'ERROR: Cono de vision no esta en forma standard'
        #    print 'Generado por agente: ',self.id
        #    print 'Posicion :',self.pos
        #    print 'Puntos de poligono de vision: ',points_of_polygon(walls,1)
        #    return -1
        
        # Guardo poligono de vision
        self.vis_polygon = walls
        
        # Devuelvo el cono de vision
        return walls
       
    
    # D: Devuelve la posicion del agente con la escala aplicada
    def posEscala(self):
        # Devuelvo la posicion del agente con la escala aplicada
        return self.escala*self.pos[0], self.escala*self.pos[1]
    
    # Desc: Calcula el gradiente del potencial y lo devuelve en coordenadas
    def gradiente_potencial_morse(self):
        global GLOBAL_VAR, agentes
        # Cargo parametros del potencial
        C_a = GLOBAL_VAR['C_a']
        l_a =GLOBAL_VAR['l_a']
        C_r =GLOBAL_VAR['C_r']
        l_r =GLOBAL_VAR['l_r']
        alpha =GLOBAL_VAR['alpha']
        beta =GLOBAL_VAR['beta']
        
        # Seteo a 0 las variables donde se guardara el valor del gradiente
        gradiente_coordenada_x = 0
        gradiente_coordenada_y = 0
        
        # Calculo factor repulsion y atraccion
        f_r = C_r/l_r
        f_a = C_a/l_a
        
        # Por defecto veo todos los agentes
        grupo = agentes
        
        # Si sensor de vision enable entonces senso y actualizo grupo
        if VISILIBITY['enable']:
            # Uso sensor de vision. Actualiza lista de agentes visibles en 
            # variable self.visibles
            self.sensor()
            grupo = self.visibles

        grupo_sensado = []
        #if SENSOR['enable']:
            #''' Funcion que siente a los agentes que no veo. Tomo como sensor
            #de mayor precedencia el sensor de vista, por eso no tengo en cuenta 
            #los agentes que 'siento' cuando ya los puedo ver. De esta forma se 
            #elige que sentir a alguien que se puede ver no agrega peso a la
            #funcion. 
            #Interesa el grupo de agentes que 'siento' pero no veo pues su peso
            #es 0 y dejara de serlo
            #La funcion guarda a los agentes en un array llamado visiblesCA'''
            #self.sensarCA()
            
            #grupo_sensado = self.visiblesCA
            
        # Calculo el gradiente del potencial para todo agente distinto a 
        # mi mismo y que este en mi area de vision
        for agent in grupo:
            if agent.id == self.id:
                # Si soy yo continuo
                continue
            
            # Guardo distancia por coordenadas desde mi posicion al agente
            dist_x = self.pos[0] - agent.pos[0]
            dist_y = self.pos[1] - agent.pos[1]
            
            # Calculo el modulo de la distancia
            mod_r = math.sqrt(dist_x**2+dist_y**2)
            
            if (mod_r) < 1e-3:
                # Si la distancia es muy chica entonces no lo tomo en cuenta
                continue
            
            # Calculo los factores del potencial
            e_r = math.pow(math.e, -(mod_r)/l_r)
            e_a = math.pow(math.e, -(mod_r)/l_a)
            
            # Calculo la repelencia y atraccion
            rep = f_r*e_r
            atrac = f_a*e_a
            
            # Sumo a las coordenadas del gradiente lo correspondiente al 
            # agente que estoy teniendo en cuenta
            gradiente_coordenada_x += (-dist_x/mod_r)*( rep - atrac)
            gradiente_coordenada_y += (-dist_y/mod_r)*( rep - atrac)
        
        for agent in grupo_sensado:
            if agent.id == self.id:
                # Si soy yo continuo
                continue
            
            # Guardo distancia por coordenadas desde mi posicion al agente
            dist_x = self.pos[0] - agent.pos[0]
            dist_y = self.pos[1] - agent.pos[1]
            
            # Calculo el modulo de la distancia
            mod_r = math.sqrt(dist_x**2+dist_y**2)
            
            if (mod_r) < 1e-3:
                # Si la distancia es muy chica entonces no lo tomo en cuenta
                continue
            
            # Calculo los factores del potencial
            e_r = math.pow(math.e, -(mod_r)/l_r)
            e_a = math.pow(math.e, -(mod_r)/l_a)
            
            # Calculo la repelencia y atraccion
            rep = f_r*e_r
            atrac = f_a*e_a
            
            # Sumo a las coordenadas del gradiente lo correspondiente al 
            # agente que estoy teniendo en cuenta
            # Los peso
            gradiente_coordenada_x += SENSOR['peso']*(-dist_x/mod_r)*( rep - atrac)
            gradiente_coordenada_y += SENSOR['peso']*(-dist_y/mod_r)*( rep - atrac)
        
        # Devuelvo valor del gradiente por coordenadas
        return [gradiente_coordenada_x,gradiente_coordenada_y ]
    
    # Desc: Actualiza posicion del agente para t+dt usando 
    #       paso 1) del algoritmo de Velocity Verlet:
    #           x(t+dt) = x(t) + v(t)*dt + 0.5*a(t)*dt^2
    # Ret: Nada
    def update_position(self):
        global GLOBAL_VAR
        # Hace paso 1) del algoritmo de Velocity Verlet
        new_x = self.pos[0] + self.speed[0]*GLOBAL_VAR['dt'] + 0.5*self.acel[0]*(GLOBAL_VAR['dt']**2)
        new_y = self.pos[1] + self.speed[1]*GLOBAL_VAR['dt'] + 0.5*self.acel[1]*(GLOBAL_VAR['dt']**2)
        #self.aux_pos = (new_x, new_y)
        # Actualiza valores
        self.pos = (new_x, new_y)
    
    # Desc: Actualiza velocidad del agente para t+dt usando paso 2) del
    #       algoritmo de Velocity Verlet
    #           v(t+dt) = v(t) + 0.5* (a(t)+a(t+dt))*dt
    # Ret: Nada
    def update_velocity(self):
        global GLOBAL_VAR
        # Calculo aceleracion para t+dt
        new_acel = self.update_aceleration()
        
        # Calculo nueva velocidad para ambas coordenadas
        new_vel_x = self.speed[0] + 0.5*(self.acel[0]+new_acel[0])*GLOBAL_VAR['dt']
        new_vel_y = self.speed[1] + 0.5*(self.acel[1]+new_acel[1])*GLOBAL_VAR['dt']
        
        # Actualizo nuevos valores de velocidad y aceleracion
        self.speed = [new_vel_x ,new_vel_y ]
        self.acel = new_acel
        
    # Desc: Actualiza aceleracion utilizando ecuacion 2 del paper:
    #   'Self-Propelled Particles with Soft-Core Interactions: 
    #               Patterns, Stability, and Collapse'
    # Ret: Aceleracion por coordenadas [ax, ay]
    def update_aceleration(self):
        global GLOBAL_VAR
        
        # Cargo constantes de la ecuacion
        alpha =GLOBAL_VAR['alpha']
        beta =GLOBAL_VAR['beta']
        
        # Calculo el gradiente
        gradiente = self.gradiente_potencial_morse()
        
        # Calculo modulo de velocidad del agente
        vel = math.sqrt(self.speed[0]**2 + self.speed[1]**2)
        
        # Calculo parte de lo relacionado a la velocidad
        factor_fuerza = (alpha - beta*(vel**2))
        
        # Calculo la fuerza por coordenadas
        self.fuerza[0] = factor_fuerza*self.speed[0] - gradiente[0]
        self.fuerza[1] = factor_fuerza*self.speed[1] - gradiente[1]

        # Devuelvo la aceleracion por coordenadas
        return [self.fuerza[0]/self.masa, self.fuerza[1]/self.masa]
    
    # Deprecated
    def velocity_verlet( self):
        global GLOBAL_VAR, FLAG_DEBUGG
        # Actualizo valores de fuerza y aceleracion
        #self.gradiente_potencial_morse()
        self.aux_pos = self.pos
        new_acel = self.fuerza_posicion()
        # 2. Actualizo velocidad con medio time-step
        #dt_2 = dt/ 2.0
        new_vel_x = self.speed[0] + new_acel[0]*GLOBAL_VAR['dt']/2
        new_vel_y = self.speed[1] + new_acel[1]*GLOBAL_VAR['dt']/2
        #self.speed = [new_vel_x,new_vel_y]
        # 1. Actualizo posicion
        new_x = self.pos[0] + new_vel_x *GLOBAL_VAR['dt']
        new_y = self.pos[1] + new_vel_y *GLOBAL_VAR['dt']
        #self.pos = (new_x, new_y)
        self.aux_pos = (new_x, new_y)
        
        # 3. Actualizo aceleraciones usando gradiente
        #self.gradiente_potencial_morse()
        new_acel = self.fuerza_posicion()
        
        # 4. Actualizo velocidad con medio time-step
        new_vel_x += new_acel[0]*GLOBAL_VAR['dt']/2
        new_vel_y += new_acel[1]*GLOBAL_VAR['dt']/2
        
        self.aux_vel = [new_vel_x, new_vel_y]
        self.aux_acel = new_acel
    
    # Deprecated
    def fuerza_posicion(self):
        for agent in agentes:
            if agent.id == self.id:
                #print "Soy YO, continue"
                continue
            # Distancias con posiciones actualizadas
            #dist_x = agent.aux_pos[0] - self.aux_pos[0]
            #dist_y = agent.aux_pos[1] - self.aux_pos[1]
            dist_x = agent.pos[0] - self.pos[0]
            dist_y = agent.pos[1] - self.pos[1]
            
            r_vector = (dist_x, dist_y)
            mod_r = math.sqrt(dist_x**2 + dist_y**2)
            
            if mod_r <= 1e-3:
                r_versor = (0,0)
                print "Dist chica. Agente ",self.id
            else:
                r_versor = (r_vector[0]/mod_r, r_vector[1]/mod_r)
            cte=10
            #new_fuerza = [cte*r_versor[0], cte*r_versor[1]]
            new_fuerza = [r_vector[0]*((2*math.pi)**2),r_vector[1]*((2*math.pi)**2)]
            self.fuerza = new_fuerza
            
        return [new_fuerza[0]/self.masa, new_fuerza[1]/self.masa]
    
    # Not used right now
    def use_vision(self):
        # DESCRIP: Actualiza el vecino mas cercano y los vecinos que ve
        # RET: Devuelve la distancia al vecino mas cercano
        global GLOBAL_VAR, agentes
        min_dist = -1
        for agente in agentes:
            if agente.id == self.id:
                continue
            dist_x = agente.pos[0] - self.pos[0]
            dist_y = agente.pos[1] - self.pos[1]
            new_dist = math.sqrt(dist_x**2, dist_y**2)
            # if nueva distancia menor a min_dist (distanci min hasta ahora)
            if menor(new_dist, min_dist):
                # Entonces actualizo mi vecino mas cercano
                self.nearest = agente
                # Actualizo la nueva distancia minima a un vecino
                min_dist = new_dist
            # if nueva distancia menor o igual a rango de vision
            if menor(new_dist, GLOBAL_VAR['VISION_RANGE']):
                # Agrego a nuevo vecino por estar en mi rango de vision
                self.neighbors.append(agente)
        return min_dist
    
    # Deprecated. Not used
    def update(self):
        # usa reglas discretas para actualizar
        #dist = self.use_vision()
        if dist < GLOBAL_VAR['MIN_SEP']:
            self.avoid()
        
    # Desc: Actualiza el valor de la fuerza  y grafica al agente en 
    #       su nueva posicion de ser necesario
    # Ret: Nada
    def update_values(self):
        global GLOBAL_VAR
        # Actualizo la variable fuerza
        self.fuerza = [self.acel[0]* self.masa, self.acel[1]* self.masa]
        
        # Si estoy simulando con graficos dibujo al agente
        if GLOBAL_VAR['GRAFICO']:
            # Posiciono al agente usando la escala
            self.rect.centerx, self.rect.centery = self.posEscala()
            
    # Desc: Obtiene la posicion, velocidad y fuerza actual del agente
    # Ret: String con posicion, velocidad y fuerza separadas por una ','
    def get_data (self):
        # Defino variable a ser usa para guardar todos los datos
        ret_value = ''
        
        # Obtengo y concateno la posicion en X e Y
        ret_value += str(self.pos[0])+','+str(self.pos[1])+','
        
        # Obtengo y concateno la velocidad en X e Y
        ret_value += str(self.speed[0])+','+str(self.speed[1])+','
        
        # Obtengo y concateno la fuerza en X e Y
        ret_value += str(self.fuerza[0])+','+str(self.fuerza[1])
        
        # Devuelvo string con los datos
        return ret_value
